/* 
 * rosserial::geometry_msgs::PoseArray Test
 * Sums an array, publishes sum 
 */
 
 
#include <Servo.h> 
#include <ros.h>
#include <geometry_msgs/Vector3.h>

// Uncomment if want to use laser tilt
// #define __LASER_


#define __MANIPULATOR_

ros::NodeHandle nh;


#ifdef __LASER_ 
Servo laserTilt;
geometry_msgs::Vector3 laser_servo_angles;
ros::Publisher laser_p("laser_servo_states", &laser_servo_angles);

void update_laser_state(const geometry_msgs::Vector3& msg){
   laser_servo_angles.x = 0;
   laser_servo_angles.y = msg.y;
   laser_servo_angles.z = 0;
   laser_p.publish(&manip_servo_angles);
   laserTilt.write((int)laser_servo_angles.y);   
}

// publisher and subscribers for laser servo 
ros::Publisher laser_p("laser_servo_states", &laser_servo_angles);
ros::Subscriber<geometry_msgs::Vector3> laser_s("laser_servo_angles",update_laser_state);

#endif

#ifdef __MANIPULATOR_ 
Servo manip_servo1;
Servo manip_servo2;
Servo manip_servo3;
geometry_msgs::Vector3 manip_servo_angles;

// publisher and subscribers for laser servo 
ros::Publisher manip_p("manip_servo_states", &manip_servo_angles);

void update_manip_state(const geometry_msgs::Vector3& msg){
   manip_servo_angles.x = msg.x;
   manip_servo_angles.y = msg.y;
   manip_servo_angles.z = msg.z;
   manip_servo1.write((int)manip_servo_angles.x);   
   manip_servo2.write((int)manip_servo_angles.y);
   manip_servo3.write((int)manip_servo_angles.z);
   manip_p.publish(&manip_servo_angles);
}

ros::Subscriber<geometry_msgs::Vector3> manip_s("manip_servo_angles",update_manip_state);

#endif

Servo cameraTilt;
Servo cameraPan;
geometry_msgs::Vector3 ptu_servo_angles;
ros::Publisher ptu_p("ptu_servo_states", &ptu_servo_angles);
void update_ptu_state(const geometry_msgs::Vector3& msg){
   ptu_servo_angles.x = 0;
   ptu_servo_angles.y = msg.y;
   ptu_servo_angles.z = msg.z;
   ptu_p.publish(&ptu_servo_angles);
   delay(10);
   cameraTilt.write((int)ptu_servo_angles.y);
   cameraPan.write((int)ptu_servo_angles.z);   
    
}



ros::Subscriber<geometry_msgs::Vector3> ptu_s("ptu_servo_angles",update_ptu_state);


void initParams(){
  
  ptu_servo_angles.x = 0;    // No rotation about X-Axis in PTU 
  ptu_servo_angles.y = 90;   // Tilt angle setting 
  ptu_servo_angles.z = 90;   // Pan angle setting 
  cameraTilt.write((int)ptu_servo_angles.y);
  cameraPan.write((int)ptu_servo_angles.z);
  
#ifdef __LASER_ 

  laser_servo_angles.x = 0;
  laser_servo_angles.y = 90;   // Laser Servo is only about Y Axis 
  laser_servo_angles.z = 0;
  laserTilt.write(laser_servo_angles.y); 

#endif
#ifdef __MANIPULATOR_

  manip_servo_angles.x = 90;
  manip_servo_angles.y = 90;   // Laser Servo is only about Y Axis 
  manip_servo_angles.z = 90;
  manip_servo1.write((int)manip_servo_angles.x);   
  manip_servo2.write((int)manip_servo_angles.y);
  manip_servo3.write((int)manip_servo_angles.z); 

#endif

}


void setup()
{ 
   nh.initNode();
 
#ifdef __LASER_ 

  nh.subscribe(laser_s);
  nh.advertise(laser_p);
  laserTilt.attach(13);

#endif

#ifdef __MANIPULATOR_ 

  nh.subscribe(manip_s);
  nh.advertise(manip_p);
  manip_servo1.attach(10);
  manip_servo2.attach(11);
  manip_servo3.attach(12);

#endif

  nh.subscribe(ptu_s);
  nh.advertise(ptu_p);
   TIMSK0 = 0x00 ;
  cameraPan.attach(8);
  cameraTilt.attach(9);
  initParams();


}

void loop()
{  
  //ptu_p.publish(&ptu_servo_angles);  
  nh.spinOnce(); 
  //delay(25);
}
